/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/opSrc/operation_pallet_identify.cpp
 * @Description  : 实现了一个用于托盘识别的操作类
 *                 主要功能包括初始化、准备开始、开始操作、结束操作和运行操作等方法。
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-27 19:11:22
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include "operation_pallet_identify.h"
#include <vector>
#include <std_srvs/srv/trigger.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include <ros_callbacks.h>
#include <array>
#include <agv_msgs/msg/agv_event.hpp>
#include "public_helper.h"
int OperationPalletIdentify::exe_id_ = 0;

/**
 * @brief OperationPalletIdentify 类的构造函数
 *
 * 初始化 OperationPalletIdentify 对象，并进行一些初始化操作。
 *
 * @param msg_action agv_msgs::msg::Action 类型的消息，包含执行的动作信息
 */
OperationPalletIdentify::OperationPalletIdentify(const agv_msgs::msg::Action& msg_action)
    : Operation(msg_action),
      start_id_(0),
      end_id_(0)
{
    if (++exe_id_ > 65535) 
        exe_id_ = 0;

    DEBUG_OUT("pallet identify exe_id_=" << exe_id_);
}

/**
 * @brief 初始化操作托盘识别模块
 *
 * 该函数用于初始化操作托盘识别模块，包括参数设置、验证等。
 *
 * @return 如果初始化成功，返回true；否则返回false。
 */
bool OperationPalletIdentify::Init() 
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    SetTaskError(agv_msgs::msg::AGVEvent::PALLET_RECOGNITION_FAILED, "no error", agv_msgs::msg::AGVEvent::NONE, false);
    result_ = ExeResult::Ready;

    work_depth_ = ROSNode()->get_parameter_or("find_rack_center.center_depth", -1.0f);
    if (work_depth_ < 0) {
        DEBUG_ERROR_OUT("error: pallet identify center_depth is not set or < 0");
        return false;
    }

    client_ = ROSNode()->create_client<agv_srvs::srv::PalletRecognition>("/pallet_recognition");

    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        DEBUG_ERROR_OUT("Interrupted while waiting for the service. Exiting.");
        return false;
      }
      DEBUG_OUT("pallet_recognition service not available, waiting again...");
    }

    return ValidateParam();
}


/**
 * @brief 准备开始托盘识别操作
 *
 * 准备开始托盘识别操作。在调试模式下，直接返回true；否则，调用GetRawPathCallBack函数获取路径信息。
 *
 * @return 如果成功准备开始托盘识别操作，返回true；否则返回false。
 */
bool OperationPalletIdentify::ReadyStart() 
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    DEBUG_OUT("pallet identify ReadyStart");
    if (!GetRawPathCallBack) {
        DEBUG_ERROR_OUT("error: pallet identify GetRawPathCallBack is null");
        return false;
    }
    bool ret = GetRawPathCallBack(start_id_, end_id_, raw_pos_);
    if (!ret) {
        DEBUG_ERROR_OUT("error: pallet identify GetRawPathCallBack failed");
        return false;
    }
    DEBUG_OUT("GetRawPathCallBack OK");
    return ret;
}

/**
 * @brief 开始执行操作
 *
 * 此函数用于启动操作托盘识别模块的执行动作。
 *
 * @return 如果成功启动操作，则返回true；否则返回false。
 */
bool OperationPalletIdentify::StartAction() 
{
    std::lock_guard<std::mutex> locker(mutex_);
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    bool ret = Recognize(ReqType::START, exe_id_, raw_pos_[0], work_depth_);
    return ret;
}

/**
 * @brief 执行托盘识别操作
 *
 * 根据当前的状态执行托盘识别操作，并返回操作结果。
 *
 * @return 如果操作成功返回true，否则返回false。
 */
bool OperationPalletIdentify::Work()
{
    std::lock_guard<std::mutex> locker(mutex_);
#ifdef AGV_DEBUG_NAV
    return total_count_++ > 100; // 5s-500
#endif
    switch (result_) {
        case ExeResult::Ready: {
            const auto& op_data = GetOpData();
            auto result = op_data.pallet_result_msg;
            if (!result.IsNew(150)) {
                DEBUG_ERROR_OUT("pallet identify no new result; data time out");
                return false;
            }
            auto data = result.Get();
            if (!data.finish) {
                DEBUG_ERROR_OUT("pallet identify not finish: id=" << data.id);
                return false;
            }
            if (data.id != OperationPalletIdentify::exe_id_) {
                DEBUG_ERROR_OUT("pallet identify id not match: exe_id_=" << exe_id_ << ", data.id=" << data.id);
                return false;
            }
            std::array<Point, 4> points;
            points[0] = FromGeometryPose2D(data.startpnt);
            points[1] = FromGeometryPose2D(data.ctrlpnt1); 
            points[2] = FromGeometryPose2D(data.ctrlpnt2); 
            points[3] = FromGeometryPose2D(data.endpnt);
            if (!ReplacePalletEndPathCallBack) {
                DEBUG_ERROR_OUT("error: pallet identify ReplacePalletEndPathCallBack is null");
                return false;
            }
            bool ret = ReplacePalletEndPathCallBack(start_id_, end_id_, points);
            if (!ret) {
                result_ = ExeResult::Failed;
                DEBUG_ERROR_OUT("error: pallet identify ReplacePalletEndPathCallBack failed");
                return false;
            }
            result_ = ExeResult::Ok;
            DEBUG_OUT("ReplacePalletEndPathCallBack: ok; result:"
                    << "points[0]=" << "(" << points[0].x_ << "," << points[0].y_ << "),"
                    << "points[1]=" << "(" << points[1].x_ << "," << points[1].y_ << "),"
                    << "points[2]=" << "(" << points[2].x_ << "," << points[2].y_ << "),"
                    << "points[3]=" << "(" << points[3].x_ << "," << points[3].y_ << ")");
            return true;
        }
        case ExeResult::Ok:
            return true;
        case ExeResult::Failed:
            SetTaskError(agv_msgs::msg::AGVEvent::PALLET_RECOGNITION_FAILED, "pallet recognition failed", agv_msgs::msg::AGVEvent::ALARM);
            return false;
        default:
            break;
    }
    return false;
}

/**
 * @brief 结束操作动作
 *
 * 根据编译选项 AGV_DEBUG_NAV 的值，决定是否直接返回 true，否则调用 Recognize 函数进行动作识别，
 * 并返回其返回值。
 *
 * @return 如果编译选项 AGV_DEBUG_NAV 被定义，返回 true；否则返回 Recognize 函数的返回值。
 */
bool OperationPalletIdentify::EndAction() 
{
#ifdef AGV_DEBUG_NAV
    return true;
#endif
    bool ret = Recognize(ReqType::STOP, exe_id_, raw_pos_[0], work_depth_);
    DEBUG_OUT("OperationWaitForRelease EndAction ok; ret=" << ret);
    return ret;
}

/**
 * @brief 验证操作参数
 *
 * 该函数用于验证操作参数是否有效。
 *
 * @return 如果参数有效返回true，否则返回false。
 */
bool OperationPalletIdentify::ValidateParam() 
{
    std::array<bool, 2> rets = {false, false};
    auto params = action_.actionparameters;
    for (const auto& iter : params) {
        if (iter.key == KEY_START_ID) {
            if (!string2Number(iter.value, start_id_)) {
                DEBUG_ERROR_OUT("error: to number failed; start_id_ : value:" << iter.value);
            } else {
                rets[0] = true;
                DEBUG_OUT("to number ok: start_id_=" << start_id_);
            }
        } else if (iter.key == KEY_END_ID) {
            if (!string2Number(iter.value, end_id_)) {
                DEBUG_ERROR_OUT("error: to number failed; end_id_ : value:" << iter.value);
            } else {
                rets[1] = true;
                DEBUG_OUT("to number ok: end_id_=" << end_id_);
            }
        }
    }
    bool ok = rets[0] && rets[1];
    if (!ok) {
        DEBUG_ERROR_OUT("error: param miss; rets[0]=" << static_cast<int>(rets[0]) << ", rets[1]=" << static_cast<int>(rets[1]));
    }
    return ok;
}

/**
 * @brief 根据给定的模式和托盘ID识别托盘操作
 *
 * 根据提供的模式、托盘ID、起始姿态和工作深度，调用服务来识别托盘操作。
 *
 * @param mode 操作模式
 * @param id 托盘ID
 * @param start_pose 起始姿态
 * @param work_depth 工作深度
 *
 * @return 如果识别成功返回true，否则返回false
 */
bool OperationPalletIdentify::Recognize(int mode, int id, const Posture& start_pose, float work_depth) 
{
    auto start_pose_2d = ToGeometryPose2D(start_pose);
    auto request = std::make_shared<agv_srvs::srv::PalletRecognition::Request>();
    request->mode = mode;
    request->id = id;
    request->startpose = start_pose_2d;
    request->workdepth = work_depth;

    using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::PalletRecognition>::SharedFuture;
    auto response_received_callback = [mode](ServiceResponseFuture future) {
        if (future.get()->state)  
            DEBUG_OUT("pallet identify Recognize mode" << mode << " success");
        else
            DEBUG_ERROR_OUT("pallet identify Recognize mode"
                  << mode << " failed; msg=" << future.get()->message);
    };

    auto result = client_->async_send_request(request, response_received_callback);
  
    if (result.valid()) {
        DEBUG_OUT("Request successfully sent");
        return true;
    } else {
        DEBUG_OUT("Failed to send request");
        return false;
    }
}

/**
 * @brief 将自定义的 Posture 结构体转换为 geometry_msgs::msg::Pose2D 类型
 *
 * 将自定义的 Posture 结构体转换为 geometry_msgs::msg::Pose2D 类型，以便在 ROS 消息传递中使用。
 *
 * @param pos 自定义的 Posture 结构体，包含位置（x, y）和方向（theta）信息
 * @return 转换后的 geometry_msgs::msg::Pose2D 结构体
 */
geometry_msgs::msg::Pose2D OperationPalletIdentify::ToGeometryPose2D(const Posture& pos) 
{
    geometry_msgs::msg::Pose2D pose_2d;
    pose_2d.x = pos.x_;
    pose_2d.y = pos.y_;
    pose_2d.theta = pos.theta_;
    return pose_2d;
}

/**
 * @brief 将Point对象转换为geometry_msgs::msg::Pose2D对象
 *
 * 将传入的Point对象转换为geometry_msgs::msg::Pose2D对象，其中Pose2D对象的x和y坐标与Point对象的x_和y_坐标相同，
 * theta角度设置为0。
 *
 * @param point Point对象，包含x_和y_坐标
 * @return 转换后的geometry_msgs::msg::Pose2D对象
 */
geometry_msgs::msg::Pose2D OperationPalletIdentify::ToGeometryPose2D(const Point& point) 
{
    geometry_msgs::msg::Pose2D pose_2d;
    pose_2d.x = point.x_;
    pose_2d.y = point.y_;
    pose_2d.theta = 0;
    return pose_2d;
}

/**
 * @brief 将给定的二维位姿（geometry_msgs::msg::Pose2D）转换为点（Point）
 *
 * 该函数接受一个geometry_msgs::msg::Pose2D类型的参数，并将其转换为Point类型的对象。
 *
 * @param pose 输入的二维位姿，包含x和y坐标。
 * @return 转换后的Point对象，包含从pose中提取的x和y坐标。
 */
Point OperationPalletIdentify::FromGeometryPose2D(const geometry_msgs::msg::Pose2D& pose) 
{
    Point point;
    point.x_ = pose.x;
    point.y_ = pose.y;
    return point;
}